#!/usr/bin/env python
# -*- coding: UTF-8 -*-
'''
@Project ：bignet_sumo_dev
@File ：main-dev-tongluoshan.py
@IDE ：PyCharm
@Author ：xinyingjie
@Date ：2025/3/24 9:40
'''

from __future__ import absolute_import
from __future__ import print_function
import math
import queue
import json
import threading
import random
import time
from multiprocessing import Process, Queue

import traci.constants as tc
import sumolib

from kafka import KafkaProducer, KafkaConsumer
from pyproj import Proj

from config.config import GLOBAL_POLYGON_CENTER, MAX_SPEED, KAFKA_HOST, KAFKA_RECEIVE_TOPIC, \
    RADAR_DATA_LIST, KAFKA_SEND_TOPIC, KAFKA_E1_TOPIC, KAFKA_E1_TOPIC_SIMU, SHOW_SIMU, USE_LIBSUMO, USE_GUI
from utils import sumo_utils
from loguru import logger
from utils.tools import calculate_distance, find_vehicles_by_duration_optimized, \
    calculate_vehicle_acceleration_deceleration, get_timestamp_milliseconds
import concurrent.futures

if USE_LIBSUMO:
    import libsumo as traci
else:
    import traci

logger.add("debug.log", format="{time} {level} {message}", level="DEBUG")


def read_trafficE1_channel(target_channel, q_r, q_s, p_id, kafka_queue):
    kafka_producer = KafkaProducer(bootstrap_servers=KAFKA_HOST, key_serializer=str.encode,
                                   value_serializer=lambda x: json.dumps(x).encode('utf-8'),
                                   compression_type='gzip')
    # 实时接收全域E1帧数据
    kafka_consumer_E1 = KafkaConsumer(
        KAFKA_E1_TOPIC,
        bootstrap_servers=KAFKA_HOST,
        auto_offset_reset='latest',
        enable_auto_commit=False
    )
    logger.info(f"全域E1 {target_channel} 监听已就绪")
    count_add = 0
    RADAR_sn_set = set(RADAR_DATA_LIST.keys())
    # 存储每一辆车的多帧信息
    per_car_info_dict = {}
    sn_car_info_dict = {}
    for sn in RADAR_sn_set:
        per_car_info_dict[sn] = {}
        sn_car_info_dict[sn] = {}
    simu_json_list_before = []
    start_time = int(time.time() * 1000)
    while True:
        time.sleep(0.1)
        send_time = 0
        send_count = 0
        frame_num = 0
        for message in kafka_consumer_E1:
            if message:
                # try:
                frame_num += 1
                sn_set = set()
                one_list = []
                message_value = message.value.decode('utf-8')
                data = json.loads(message_value)
                ts_m = int(time.time() * 1000)
                if abs(ts_m - start_time) > 1000 * 120:
                    start_time = ts_m
                    kafka_queue.put([target_channel, int(time.time())])
                e1FrameParticipant_list = data['e1FrameParticipant']

                ts = get_timestamp_milliseconds(data['globalTimeStamp'])

                for one in e1FrameParticipant_list:
                    sourceId = one['sourceId']
                    if sourceId % 2 != 0:
                        sn_set.add(sourceId + 1)
                    else:
                        sn_set.add(sourceId)
                    # if sourceId != 34:
                    if sourceId % 2 != 0:
                        one['id'] = one['id'].split("-")[1][-5:]
                        dis = (calculate_distance(RADAR_DATA_LIST[sourceId + 1]['lon'],
                                                  RADAR_DATA_LIST[sourceId + 1]['lat'],
                                                  one['longitude'], one['latitude']))
                        if dis <= 100:
                            one_list.append(one)
                        continue

                    car_id = one['id']
                    dis = (calculate_distance(RADAR_DATA_LIST[sourceId]['lon'],
                                              RADAR_DATA_LIST[sourceId]['lat'],
                                              one['longitude'], one['latitude']))
                    if dis <= 100:
                        if car_id not in per_car_info_dict[sourceId].keys():
                            per_car_info_dict[sourceId][car_id] = {}
                            per_car_info_dict[sourceId][car_id][ts] = one
                        else:
                            per_car_info_dict[sourceId][car_id][ts] = one
                        one['row_id'] = one['id']
                        one['id'] = one['id'].split("-")[1][-5:]
                        one_list.append(one)

                continuous_vehicles, disappeared_vehicles, delete_vehicles = find_vehicles_by_duration_optimized(
                    per_car_info_dict, 200, 1000, 50000)
                for sn, disappeared_vehicle_list in disappeared_vehicles.items():
                    for disappeared_vehicle in disappeared_vehicle_list:
                        dis_id = disappeared_vehicle[0]
                        # print(list(per_car_info_dict[sn][dis_id].keys())[-1] - int(time.time()* 1000))
                        if dis_id not in sn_car_info_dict[sn].keys():
                            sn_car_info_dict[sn][dis_id] = disappeared_vehicle[3]
                            car_one = disappeared_vehicle[3]
                            car_one['id'] = str(car_one['row_id']).split('-')[1] + "-" + str(car_one['sourceId'])
                            # logger.info(f"新增车辆{car_one}")
                            disappeared_vehicle[3]['error_sn'] = list(RADAR_sn_set - sn_set)

                            count_add += 1
                            logger.info(f"新增车辆总数{count_add}")
                            q_r.put(disappeared_vehicle[3])

                for c_id, s_id in delete_vehicles.items():
                    # logger.info(f"删除车辆{c_id} {per_car_info_dict[s_id].keys()}  {sn_car_info_dict[s_id].keys()}")
                    if c_id in per_car_info_dict[s_id].keys():
                        del per_car_info_dict[s_id][c_id]
                        # logger.info(f"per_car_info_dict 删除成功{c_id}")
                    if c_id in sn_car_info_dict[s_id].keys():
                        del sn_car_info_dict[s_id][c_id]
                        # logger.info(f"sn_car_info_dict 删除成功{c_id}")
                simu_json_list = []
                while not q_s.empty():
                    simu_json_list = q_s.get()
                    logger.info(f"发送端{len(simu_json_list)}")
                if len(simu_json_list) > 0:
                    simu_json_list_before = simu_json_list
                if SHOW_SIMU:
                    data['e1FrameParticipant'] = one_list + simu_json_list_before if len(
                        simu_json_list_before) > 0 else one_list
                else:
                    data['e1FrameParticipant'] = one_list
                # logger.info(f"感知数量为{len(one_list)} 仿真数量为{len(self.simu_json_list_before)}")

                kafka_producer.send(KAFKA_E1_TOPIC_SIMU, value=data,
                                    key=str(int(time.time())))
                error_sn_set = RADAR_sn_set - sn_set
                # logger.error(f"故障雷达{error_sn_set}")
                for sn in RADAR_sn_set:
                    if sn in error_sn_set:
                        RADAR_DATA_LIST[sn]['normal'] = False
                    else:
                        RADAR_DATA_LIST[sn]['normal'] = True
                end_time = time.time()
                if end_time - send_time > 1:
                    logger.info(f"进程{p_id} 发送帧率 {abs(send_count - frame_num)}  加车队列长度{q_r.qsize()} 发送队列长度{q_s.qsize()}")
                    send_time = end_time
                    send_count = frame_num = 0

                    # self.kafka_producer.send(KAFKA_E1_TOPIC_SIMU, value=data,
                    #                          key=str(int(time.time())))
                # except Exception as e:
                #     logger.error(str(e))


class SumoMain:
    def __init__(self, sumo_net_file, sumo_cfg_file, gui, process_id):
        """
        仿真初始化
        :param sumo_net_file:  仿真路网文件路径
        :param sumo_cfg_file:  仿真配置文件路径
        :param gui: 是否开启gui
        :param process_id: 进程id
        """

        self.sumo_net_file = sumo_net_file
        self.sumo_cfg_file = sumo_cfg_file
        self.gui = gui
        self.process_id = process_id
        # 仿真路网对象
        self.sumo_net_instance = None
        # 存储卡口、雷达经纬度所在路网的路段和位置信息
        self.kakou_pos_dict = {}
        self.init_sumo()
        self.simu_json_list = []
        self.simu_json_list_before = []

        self.sn_car_info_dict = {}
        self.RADAR_sn_set = set(RADAR_DATA_LIST.keys())
        # 存储每一辆车的多帧信息
        self.per_car_info_dict = {}
        for sn in self.RADAR_sn_set:
            self.sn_car_info_dict[sn] = {}
            self.per_car_info_dict[sn] = {}
        self.add_car_station_info = {}

        # 是否开启速度控制
        self.speed_control = False
        # 正在加速列表
        self.speed_accelerate_list = []
        # 正在减速列表
        self.speed_decelerate_list = []

        self.executor = concurrent.futures.ThreadPoolExecutor(max_workers=300)
        self.global_vehicles = {}
        self.batch_num = 0
        self.global_time = time.time()
        self.global_num = 0
        self.son_process = None
        self.add_count = 0

    def init_sumo(self):
        """
        初始化sumo  加载
        :return:
        """
        # 加载路网
        self.sumo_net_instance = sumolib.net.readNet(self.sumo_net_file, withInternal=True, withLatestPrograms=True)
        # 启动仿真
        sumo_utils.start_sumo(self.sumo_cfg_file, False, gui=self.gui)
        # 设置所有车道的最大通行速度
        for id in traci.lane.getIDList():
            traci.lane.setMaxSpeed(id, MAX_SPEED)
        # 新增区域中心矩形，用于订阅100KM范围内的所有仿真车辆信息，并配置监听信息内容
        traci.polygon.add('GLOBAL_POLYGON_CENTER', GLOBAL_POLYGON_CENTER, (1, 0, 1))
        traci.polygon.subscribeContext('GLOBAL_POLYGON_CENTER',
                                       tc.CMD_GET_VEHICLE_VARIABLE, 1000000,
                                       [tc.VAR_SPEED, tc.VAR_TYPE, tc.VAR_POSITION, tc.VAR_ANGLE, ])

        # 将雷达、卡口相机的经纬度和终点位置转为路网上所在的路段id 位置距离 车道号，起始点和终点所在的路段id添加路由
        for sn, sn_info in RADAR_DATA_LIST.items():
            # 转换经纬度到路网坐标
            center_x, center_y = traci.simulation.convertGeo(sn_info['lon'], sn_info['lat'], fromGeo=True)
            EdgeID, Pos, LaneIndex = traci.simulation.convertRoad(sn_info['lon'], sn_info['lat'], True)
            next_edge_id, next_pos = sumo_utils.get_downstream_position_simplified(self.sumo_net_instance, EdgeID, Pos,
                                                                                   100)
            # arrive_edge_id, arrive_pos = sumo_utils.get_downstream_position_simplified(self.sumo_net_instance, EdgeID,
            #                                                                            Pos, 1850)
            # arrive_edge_id, arrive_pos = sumo_utils.get_downstream_position_simplified(self.sumo_net_instance, EdgeID,
            #                                                                            Pos, 150)

            logger.info(f"门架{sn_info['sn']} 所在路段为{EdgeID} 位置为{Pos} 下行100米后的路段为{next_edge_id} 位置为{next_pos}")
            if sn_info['sn'] not in self.kakou_pos_dict.keys():
                traci.route.add(str(sn_info['sn']), [str(next_edge_id), '-15'])
                self.kakou_pos_dict[str(sn_info['sn'])] = {"EdgeID": next_edge_id, "Pos": next_pos,
                                                           'LaneIndex': LaneIndex,
                                                           'routeId': str(str(sn_info['sn']))}
            # 创建圆形区域，表示相机或雷达的位置，用于在使用gui可视化时，展示设备具体位置
            if sn_info['normal']:
                # 创建圆形区域
                self.create_circle_polygon(sn, center_x, center_y, radius=50)
            else:
                self.create_circle_polygon(sn, center_x, center_y, radius=50, color=(255, 0, 0, 255))

    def create_circle_polygon(self, vehID, center_x, center_y, radius, color=(0, 255, 0, 255), filled=True):
        """
        在指定位置创建一个近似圆形的多边形。

        :param vehID: 车辆 ID 或其他唯一标识符，用于命名多边形
        :param center_x: 圆心的 x 坐标
        :param center_y: 圆心的 y 坐标
        :param radius: 圆的半径
        :param color: 多边形的颜色 (R,G,B,A)，默认绿色不透明
        :param filled: 是否填充多边形，默认 True
        """
        # 生成圆周上的点
        num_points = 50  # 点的数量越多，圆越光滑
        points = []
        for i in range(num_points):
            angle = 2 * math.pi * i / num_points
            x = center_x + radius * math.cos(angle)
            y = center_y + radius * math.sin(angle)
            points.append((x, y))

        # 添加多边形
        polygon_id = f"circle_{vehID}"
        traci.polygon.add(polygon_id, shape=points, color=color, fill=filled, layer=0)

    def control_car(self):
        self.add_count += re_queue_data.qsize()
        logger.info(f"加车队列数量{self.add_count}")
        # vehiclePicInfo = None
        while not re_queue_data.empty():
            vehiclePicInfo = re_queue_data.get()
            if vehiclePicInfo:
                sn = vehiclePicInfo['sourceId']
                self.add_camera_car(vehiclePicInfo, sn)

    def add_camera_car(self, vehiclePicInfo, sn):

        # try:
        car_id = vehiclePicInfo['id']
        set_speed = int(vehiclePicInfo['speed'] / 100 if (40 / 3.6) < vehiclePicInfo[
            'speed'] / 100 < (140 / 3.6) else 120 / 3.6)
        try:
            _, _, LaneIndex = traci.simulation.convertRoad(vehiclePicInfo['longitude'], vehiclePicInfo['latitude'],
                                                           True)
            traci.vehicle.add(
                vehID=str(car_id),
                routeID=self.kakou_pos_dict[str(sn)]['routeId'],
                departPos=self.kakou_pos_dict[str(sn)]['Pos'],
                typeID=1, depart='0',
                departSpeed=str(set_speed),
                departLane=str(LaneIndex))
        except Exception as e:
            logger.info(f"加车错误{e}")
            return
        traci.vehicle.setSpeed(str(car_id),
                               set_speed)

        vehiclePicInfo['id'] = str(car_id)
        vehiclePicInfo['sourceId'] = sn
        traci.vehicle.setParameter(str(car_id), "etc_info",
                                   str(vehiclePicInfo))
        # except Exception as e:
        #     logger.error(f"加车异常{str(e)}")

    def stop_remove_car(self, global_vehicles, queue_data):
        simu_json_list = []
        for vehicle_id, veh_info in dict(global_vehicles).items():
            pos = veh_info[tc.VAR_POSITION]
            longitude, latitude = traci.simulation.convertGeo(pos[0], pos[1], fromGeo=False)
            sourceId = int(vehicle_id.split("-")[1])
            try:
                etc_info = eval(traci.vehicle.getParameter(vehicle_id, "etc_info"))

            except Exception as e:
                # logger.info("etc_info", e)
                pass
            if sourceId in RADAR_DATA_LIST.keys():
                target_next_id = None
                while RADAR_DATA_LIST[sourceId]['Downstream'] is not None:
                    # print(RADAR_DATA_LIST[sourceId]['Downstream']['sn'] in  etc_info['error_sn'].copy())
                    if RADAR_DATA_LIST[sourceId]['Downstream']['sn'] not in etc_info['error_sn']:
                        target_next_id = RADAR_DATA_LIST[sourceId]['Downstream']['sn']
                        break
                    sourceId = RADAR_DATA_LIST[sourceId]['Downstream']['sn']
                if target_next_id is not None:

                    dis_next_sn = calculate_distance(longitude, latitude,
                                                     RADAR_DATA_LIST[target_next_id]['lon'],
                                                     RADAR_DATA_LIST[target_next_id]['lat'])
                    if dis_next_sn <= 100:
                        try:

                            traci.vehicle.remove(vehicle_id)
                        except Exception as e:
                            # logger.info("stop",e)
                            pass

            else:
                logger.error(f"雷达 {sourceId}不在配置文件内")

            speed = veh_info[tc.VAR_SPEED]
            vehicle_type = veh_info[tc.VAR_TYPE]
            courseAngle = veh_info[tc.VAR_ANGLE]

            laneNum = 0
            temp = {"id": etc_info['id'].split("-")[0][-5:], "type": etc_info['type'], 'source': 4,
                    "laneNum": etc_info['laneNum'],
                    "longitude": longitude, "latitude": latitude, "speed": int(speed * 100),
                    "courseAngle": round(courseAngle, 1),
                    "length": etc_info['length'], "width": etc_info['width'], "height": etc_info['height'],
                    "sourceId": etc_info['sourceId'],
                    "plate": etc_info['plate'], "color": etc_info['color'],
                    "vehicleType": etc_info['vehicleType']
                    }
            simu_json_list.append(temp)
        logger.info(f"仿真端{len(simu_json_list)}")
        queue_data.put(simu_json_list)

    def main_loop(self, stop_data, time_data):
        global re_queue_data

        kafka_queue = Queue(maxsize=2000)
        # 在子进程中初始化多进程队列和资源
        queue_data = Queue(maxsize=2000)  # 多进程队列在此处创建
        # Process(target=send_kafka_1, args=(queue_data, self.process_id,)).start()
        # 在子进程中初始化多进程队列和资源
        re_queue_data = Queue(maxsize=2000)  # 多进程队列在此处创建
        self.son_process = Process(target=read_trafficE1_channel,
                                   args=(KAFKA_E1_TOPIC, re_queue_data, queue_data, self.process_id, kafka_queue),
                                   daemon=True)
        self.son_process.start()
        kafka_time_dict = int(time.time())
        low_frame_num = 0
        batch_num = 0
        all_set = set()

        while True:
            # try:
            start_time = time.time()
            start_time_m = int(time.time() * 1000)

            # ================================收到门架上报数据，进行车辆处理==========================================
            global_vehicles = traci.polygon.getContextSubscriptionResults('GLOBAL_POLYGON_CENTER')
            self.control_car()
            self.stop_remove_car(global_vehicles, queue_data)
            self.batch_num = batch_num
            batch_num += 1
            traci.simulationStep()
            end_time = time.time()
            all_set.update(set(global_vehicles.keys()))
            logger.info(f"仿真数量总数{len(all_set)}")
            if end_time - self.global_time > 1:
                while kafka_queue.qsize() > 0:
                    kafka_info = kafka_queue.get()
                    kafka_topic = kafka_info[0]
                    ts = kafka_info[1]
                    kafka_time_dict = ts
                if abs(kafka_time_dict - end_time) > 300:
                    logger.info(f"kafka接收进程,超过5分钟未收到数据，重启服务")
                    self.son_process.terminate()
                    self.son_process.join()
                    self.son_process = Process(target=read_trafficE1_channel,
                                               args=(
                                                   KAFKA_E1_TOPIC, re_queue_data, queue_data, self.process_id,
                                                   kafka_queue),
                                               daemon=True)
                    self.son_process.start()

                time_data.put(int(time.time()))
                simu_frame = abs(self.global_num - batch_num)
                logger.info(f"进程{self.process_id} 仿真帧率 {simu_frame} 仿真数量{len(global_vehicles)}")
                if simu_frame < 7:
                    low_frame_num += 1
                if low_frame_num > 10 or (batch_num > 2000 and len(global_vehicles) == 0):
                    logger.info(f"关闭子进程")
                    self.son_process.terminate()
                    self.son_process.join()
                    stop_data.put(1)
                self.global_time = end_time
                self.global_num = batch_num

            if start_time and (end_time - start_time) < 0.1:
                time.sleep(0.1 - (end_time - start_time))
            else:
                if start_time:
                    pass
            # except Exception as e:
            #     logger.error(f"主循环异常{str(e)}")
            #     continue


def run_sumo_simulation(sumo_net_file, sumo_cfg_file, gui, process_id, stop_data, time_data):
    sumo_main = SumoMain(sumo_net_file, sumo_cfg_file, gui, process_id)
    sumo_main.main_loop(stop_data, time_data)


if __name__ == '__main__':
    # 路网文件
    sumo_net_file = "./net/henan.net.xml"
    # 配置文件
    sumo_cfg_file = "./net/henan.sumocfg"
    # 是否启用gui可视化界面，调试阶段启用，线上部署关闭减少资源占用
    stop_data = Queue(maxsize=2000)  # 创建队列
    time_data = Queue(maxsize=2000)  # 创建队列
    gui = USE_GUI
    for i in range(1):
        logger.info("开启线程")
        tess_process = Process(target=run_sumo_simulation,
                               args=(sumo_net_file, sumo_cfg_file, gui, i, stop_data, time_data))
        tess_process.start()
        time_simu = int(time.time())
        while True:
            stop_flag = False
            while time_data.qsize() > 0:
                time_simu = time_data.get()
            if time_simu:
                logger.info(f"时间差{abs(time_simu - int(time.time()))}")
                if abs(time_simu - int(time.time())) > 60:
                    stop_flag = True
                    time_simu = int(time.time())
            while stop_data.qsize() > 0:
                _ = stop_data.get()
                stop_flag = True
            if stop_flag:
                logger.info("关闭开启线程")
                tess_process.terminate()
                tess_process.join()
                logger.info("重新开启线程")
                tess_process = Process(target=run_sumo_simulation,
                                       args=(sumo_net_file, sumo_cfg_file, gui, i, stop_data, time_data))
                tess_process.start()
            time.sleep(1)
